#include "bsp_can.h"
#include "can.h"
#include "pid.h"
#include "remote.h"
 extern  float yaw,last_yaw,offset_yaw,yaw_round_cnt;
 extern float total_yaw;
  extern  RC_Ctl_t RC_CtrlData;
moto_measure_t  moto_holder[6]={0};
float ch2_RE,ch3_RE;
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
  CAN_RxHeaderTypeDef rx_header;
  uint8_t             rx_data[8];
	if(hcan->Instance == CAN1)
 {
    HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, rx_data); //receive can data
  }

	
	if ((rx_header.StdId >= 0x201)&&(rx_header.StdId <(0x201+6)))
	{
		
	  uint8_t index = rx_header.StdId - 0x201; 
		moto_holder[index].last_angle =moto_holder[index].angle ;
	  moto_holder[index].angle    = (uint16_t)((rx_data[0] << 8) | rx_data[1]);//获取角度
    moto_holder[index].real_current    = ((rx_data[2] << 8) | rx_data[3]);//获取转速
		moto_holder[index].given_current  = ((rx_data[4] << 8) | rx_data[5]);//电流
    moto_holder[index].hall          =   rx_data[6];	

    }
}
void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
  CAN_RxHeaderTypeDef rx_header;
  uint8_t             rx_data[8];
	if(hcan->Instance == CAN2)
  {
    HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO1, &rx_header, rx_data); //receive can data
  }
	
	if ((rx_header.StdId >= 0x205)&&(rx_header.StdId <(0x201+6)))
	{
		
	  uint8_t index = rx_header.StdId - 0x201; 
		moto_holder[index].last_angle =moto_holder[index].angle ;
	  moto_holder[index].angle    = (uint16_t)((rx_data[0] << 8) | rx_data[1]);//获取角度
    moto_holder[index].real_current    = ((rx_data[2] << 8) | rx_data[3]);//获取转速
		moto_holder[index].given_current  = ((rx_data[4] << 8) | rx_data[5]);//电流
    moto_holder[index].hall          =   rx_data[6];	

    }
}
void can1_user_init(CAN_HandleTypeDef* hcan )
{
  CAN_FilterTypeDef  can_filter;
  
  uint16_t StdIdArray1[8] ={0x201,0x202,0x203,0x204,0x205,0x206,0x207,0x208};//定义第一组标准CAN ID
	uint16_t      mask,tmp,i,num;
	
	can_filter.FilterBank = 0;                       // filter 0
  can_filter.FilterMode =  CAN_FILTERMODE_IDMASK;  // mask mode
  can_filter.FilterScale = CAN_FILTERSCALE_32BIT;
  can_filter.FilterIdHigh = StdIdArray1[0]<<5;
  can_filter.FilterIdLow  = 0 ;
		
	mask =0x7ff;
  num =sizeof(StdIdArray1)/sizeof(StdIdArray1[0]);
	 for(i =0; i<num; i++)							//计算第一个屏蔽码
  {
    tmp =StdIdArray1[i] ^ (~StdIdArray1[0]);
    mask &=tmp;
  }
  can_filter.FilterMaskIdHigh = (mask<<5);
  can_filter.FilterMaskIdLow  = 0|0x02;   // set mask 0 to receive all can id
  can_filter.FilterFIFOAssignment = CAN_RX_FIFO0; // assign to fifo0
  can_filter.FilterActivation = ENABLE;           // enable can filter
  can_filter.SlaveStartFilterBank  = 14;          // only meaningful in dual can mode
   
  HAL_CAN_ConfigFilter(hcan, &can_filter);        // init can filter
  HAL_CAN_Start(&hcan1);                          // start can1
  HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING); // enable can1 rx interrupt
}


void can2_user_init(CAN_HandleTypeDef* hcan )
{
  CAN_FilterTypeDef  can_filter;
  
  uint16_t StdIdArray1[8] ={0x201,0x202,0x203,0x204,0x205,0x206,0x207,0x208};//定义第一组标准CAN ID
	uint16_t      mask,tmp,i,num;
	
	can_filter.FilterBank = 14;                       // filter 1
  can_filter.FilterMode =  CAN_FILTERMODE_IDMASK;  // mask mode
  can_filter.FilterScale = CAN_FILTERSCALE_32BIT;
  can_filter.FilterIdHigh = StdIdArray1[0]<<5;
  can_filter.FilterIdLow  = 0 ;
		
	mask =0x7ff;
  num =sizeof(StdIdArray1)/sizeof(StdIdArray1[0]);
	 for(i =0; i<num; i++)							//计算第一个屏蔽码
  {
    tmp =StdIdArray1[i] ^ (~StdIdArray1[0]);
    mask &=tmp;
  }
  can_filter.FilterMaskIdHigh = (mask<<5);
  can_filter.FilterMaskIdLow  = 0|0x02;   // set mask 0 to receive all can id
  can_filter.FilterFIFOAssignment = CAN_RX_FIFO1; // assign to fifo0
  can_filter.FilterActivation = ENABLE;           // enable can filter
  can_filter.SlaveStartFilterBank  = 14;          // only meaningful in dual can mode
   
  HAL_CAN_ConfigFilter(hcan, &can_filter);        // init can filter
  HAL_CAN_Start(&hcan2);                          // start can1
  HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO1_MSG_PENDING); // enable can1 rx interrupt
}



//void get_moto_measure(moto_measure_t *ptr, CAN_HandleTypeDef* hcan)
//{
//	ptr->last_angle = ptr->angle;
//	ptr->angle = (uint16_t)(hcan->pRxMsg->Data[0]<<8 | hcan->pRxMsg->Data[1]) ;//获取角度
//ptr->real_current  = (int16_t)(hcan->pRxMsg->Data[2]<<8 | hcan->pRxMsg->Data[3]);//获取转速

//ptr->speed_rpm = ptr->real_current;	//这里是因为两种电调对应位不一样的信息（看不懂先用着）
//ptr->given_current = (int16_t)(hcan->pRxMsg->Data[4]<<8 | hcan->pRxMsg->Data[5])/-5;//实际转矩电流
//ptr->hall = hcan->pRxMsg->Data[6];//电机温度
//	
//	if(ptr->angle - ptr->last_angle > 4096)
//		ptr->round_cnt --;
//		else if (ptr->angle - ptr->last_angle < -4096)
//		ptr->round_cnt ++;   //角度判断是否转过一圈
//		ptr->total_angle = ptr->round_cnt * 8192 + ptr->angle - ptr->offset_angle;
//}
//void get_moto_offset(moto_measure_t *ptr, CAN_HandleTypeDef* hcan)
//{
//	ptr->angle = (uint16_t)(hcan->pRxMsg->Data[0]<<8 | hcan->pRxMsg->Data[1]) ;
//	ptr->offset_angle = ptr->angle;
//}



#define ABS(x)	( (x>0) ? (x) : (-x) )
void get_total_angle(moto_measure_t *p)
{
	
	int res1, res2, delta;
	if(p->angle < p->last_angle){			//可能的情况
		res1 = p->angle + 8192 - p->last_angle;	//正转，delta=+
		res2 = p->angle - p->last_angle;				//反转	delta=-
	}else{	//angle > last
		res1 = p->angle - 8192 - p->last_angle ;//反转	delta -
		res2 = p->angle - p->last_angle;				//正转	delta +
	}
	//不管正反转，肯定是转的角度小的那个是真的
	if(ABS(res1)<ABS(res2))
		delta = res1;
	else
		delta = res2;

	p->total_angle += delta;
	p->last_angle = p->angle;
}

void set_moto_current_can1(uint8_t id_range, s16 iq1, s16 iq2, s16 iq3, s16 iq4){

  CAN_TxHeaderTypeDef tx_header;
	uint8_t             tx_data[8];
	tx_header.StdId = (id_range == 0)?(0x200):(0x1FF);
	tx_header.IDE   = CAN_ID_STD;
  tx_header.RTR   = CAN_RTR_DATA;
  tx_header.DLC   = 0x08;
  tx_data[0] = (iq1>>8)&0xff;
  tx_data[1] =    (iq1)&0xff;
  tx_data[2] = (iq2>>8)&0xff;
  tx_data[3] =    (iq2)&0xff;
  tx_data[4] = (iq3>>8)&0xff;
  tx_data[5] =    (iq3)&0xff;
  tx_data[6] = (iq4>>8)&0xff;
  tx_data[7] =    (iq4)&0xff;
	
	HAL_CAN_AddTxMessage(&hcan1, &tx_header, tx_data,(uint32_t*)CAN_TX_MAILBOX0); 
}	
void set_moto_current_can2(uint8_t id_range, s16 iq1, s16 iq2, s16 iq3, s16 iq4){

  CAN_TxHeaderTypeDef tx_header;
	uint8_t             tx_data[8];
	tx_header.StdId = (id_range == 0)?(0x200):(0x1FF);
	tx_header.IDE   = CAN_ID_STD;
  tx_header.RTR   = CAN_RTR_DATA;
  tx_header.DLC   = 0x08;
  tx_data[0] = (iq1>>8)&0xff;
  tx_data[1] =    (iq1)&0xff;
  tx_data[2] = (iq2>>8)&0xff;
  tx_data[3] =    (iq2)&0xff;
  tx_data[4] = (iq3>>8)&0xff;
  tx_data[5] =    (iq3)&0xff;
  tx_data[6] = (iq4>>8)&0xff;
  tx_data[7] =    (iq4)&0xff;
	
	HAL_CAN_AddTxMessage(&hcan2, &tx_header, tx_data,(uint32_t*)CAN_TX_MAILBOX0); 
}	
void update_angle(moto_measure_t* moto)
{
    static uint8_t init;
	if (init)
	{
	   if(moto->angle-moto->last_angle>4096)
			 moto->round_cnt--;
		 else if (moto->angle-moto->last_angle<-4096)
			 moto->round_cnt++;                                 //以上是对圈数的计算判断，若正转就是圈数++，反转就会圈数--
	 }
	else 
	{
	   moto->offset_angle=moto->angle;                        //offset_angle是将现在角度作为初始化角度，在之后的total_angle计算中会用到
		init=1;                                              
	}
     moto->  angle_360_offset=moto->offset_angle/8192.f*360.0f;           
	   moto->last_angle=moto->angle;                           //角度更新，这次作为上一次的
	   moto->total_angle=moto->round_cnt*8192+moto->angle-moto->offset_angle;    //总角度等于角度圈数*8192加上现在角度减去初始化角度
	   moto->angle_360=moto->total_angle /8192.0f*360.0f;                  //化为360度制
	
}

void update_angle_yaw_180(moto_measure_t* moto)
{  

    static uint8_t init;
	if (init)
	{
	   if(moto->angle-moto->last_angle>4096)
			 moto->round_cnt--;
		 else if (moto->angle-moto->last_angle<-4096)
			 moto->round_cnt++;                                 //以上是对圈数的计算判断，若正转就是圈数++，反转就会圈数--
	 }
	else 
	{
	   moto->offset_angle=1900;                        //offset_angle是将现在角度作为初始化角度，在之后的total_angle计算中会用到
		init=1;                                              
	}
            
	   moto->last_angle=moto->angle;                           //角度更新，这次作为上一次的
	   moto->total_angle=moto->round_cnt*8192.0f+moto->angle-moto->offset_angle;    //总角度等于角度圈数*8192加上现在角度减去初始化角度
	   moto->total_angle_180=moto->total_angle/8192.0f*360.0f;     
	
}
void update_mpu_angle()
{
    static uint8_t init;
	if(init)
	{
				if(yaw-last_yaw>180)
				yaw_round_cnt--;
				else if (yaw-last_yaw<-180)
				yaw_round_cnt++;
	}
	else 
	{
	   offset_yaw=yaw;
		  init=1;
	}
	  
	   last_yaw=yaw;
	   total_yaw=yaw_round_cnt*360.0f+yaw-offset_yaw;
	  total_yaw=total_yaw/360.0f*360.0f;
}

void set_moto_current_chassis(uint8_t id_range, s16 iq1, s16 iq2, s16 iq3, u8 iq4,u8 iq5)
{

CAN_TxHeaderTypeDef tx_header;
	uint8_t             tx_data[8];
	tx_header.StdId = (id_range == 0)?(0x222):(0x1FF);
	tx_header.IDE   = CAN_ID_STD;
  tx_header.RTR   = CAN_RTR_DATA;
  tx_header.DLC   = 0x08;
 abs_limit_s16(&iq1,1000);
	 abs_limit_s16(&iq2,1000);
	 abs_limit_s16(&iq3,4000);
  tx_data[0] = (iq1>>8)&0xff;
  tx_data[1] =    (iq1)&0xff;
  tx_data[2] = (iq2>>8)&0xff;
  tx_data[3] =    (iq2)&0xff;
  tx_data[4] = (iq3>>8)&0xff;
  tx_data[5] =    (iq3)&0xff;
  tx_data[6] =     iq4&0xff;
  tx_data[7] =     iq5&0xff;
	
	HAL_CAN_AddTxMessage(&hcan2, &tx_header, tx_data,(uint32_t*)CAN_TX_MAILBOX0); 
}

void dr16_reset()
{
   ch2_RE=(RC_CtrlData.rc.ch2-1024)*1.515f;  
	
   ch3_RE=(RC_CtrlData.rc.ch3-1024)*1.515f;  //归一化
   
	 
}